﻿// by Freya Holmér (https://github.com/FreyaHolmer/Mathfs)

using System;
using System.Runtime.CompilerServices;
using UnityEngine;

namespace Freya {

	/// <summary>A non-uniform cubic catmull-rom 2D curve</summary>
	[Serializable] public struct NUCatRomCubic2D : IParamSplineSegment<Polynomial2D, Vector2Matrix4x1> {

		public enum KnotCalcMode {
			Manual,
			Auto,
			AutoUnitInterval
		}

		const MethodImplOptions INLINE = MethodImplOptions.AggressiveInlining;

		#region Constructors

		/// <summary>Creates a cubic catmull-rom curve, from 4 control points and their corresponding knot values</summary>
		/// <param name="pointMatrix">The control points of the curve</param>
		/// <param name="knotVector">The knot vector of the curve</param>
		public NUCatRomCubic2D( Vector2Matrix4x1 pointMatrix, Matrix4x1 knotVector ) {
			this.pointMatrix = pointMatrix;
			this.knotVector = knotVector;
			validCoefficients = false;
			curve = default;
			knotCalcMode = KnotCalcMode.Manual;
			alpha = default; // unused when using manual knots
		}

		/// <summary>Creates a cubic catmull-rom curve, from 4 control points and their corresponding knot values</summary>
		/// <param name="p0">The first control point of the catrom curve. Note that this point is not included in the curve itself, and only helps to shape it</param>
		/// <param name="p1">The second control point, and the start of the catrom curve</param>
		/// <param name="p2">The third control point, and the end of the catrom curve</param>
		/// <param name="p3">The last control point of the catrom curve. Note that this point is not included in the curve itself, and only helps to shape it</param>
		/// <param name="k0">The first knot value</param>
		/// <param name="k1">The second knot value</param>
		/// <param name="k2">The third knot value</param>
		/// <param name="k3">The fourth knot value</param>
		public NUCatRomCubic2D( Vector2 p0, Vector2 p1, Vector2 p2, Vector2 p3, float k0, float k1, float k2, float k3 )
			: this( new Vector2Matrix4x1( p0, p1, p2, p3 ), new Matrix4x1( k0, k1, k2, k3 ) ) {
		}

		/// <summary>Creates a uniform cubic catmull-rom curve, from 4 control points</summary>
		/// <param name="p0">The first control point of the catrom curve. Note that this point is not included in the curve itself, and only helps to shape it</param>
		/// <param name="p1">The second control point, and the start of the catrom curve</param>
		/// <param name="p2">The third control point, and the end of the catrom curve</param>
		/// <param name="p3">The last control point of the catrom curve. Note that this point is not included in the curve itself, and only helps to shape it</param>
		public NUCatRomCubic2D( Vector2 p0, Vector2 p1, Vector2 p2, Vector2 p3 ) : this( p0, p1, p2, p3, -1, 0, 1, 2 ) {
		}

		/// <summary>Creates a cubic catmull-rom curve, from 4 control points with explicit type for auto-generating its knot values</summary>
		/// <param name="p0">The first control point of the catrom curve. Note that this point is not included in the curve itself, and only helps to shape it</param>
		/// <param name="p1">The second control point, and the start of the catrom curve</param>
		/// <param name="p2">The third control point, and the end of the catrom curve</param>
		/// <param name="p3">The last control point of the catrom curve. Note that this point is not included in the curve itself, and only helps to shape it</param>
		/// <param name="type">The type of catrom curve to use. This will internally determine the value of the <c>alpha</c> parameter</param>
		/// <param name="parameterizeToUnitInterval">If true, the knot generation will ensure k1 = 0 and k2 = 1,
		/// making it span the unit interval of 0 to 1, instead of using the raw knot values generated by the alpha parameterization</param>
		public NUCatRomCubic2D( Vector2 p0, Vector2 p1, Vector2 p2, Vector2 p3, CatRomType type, bool parameterizeToUnitInterval = true )
			: this( p0, p1, p2, p3, type.AlphaValue(), parameterizeToUnitInterval ) {
		}

		/// <summary>Creates a cubic catmull-rom curve, from 4 control points with explicit alpha parameter to define its type</summary>
		/// <param name="p0">The first control point of the catrom curve. Note that this point is not included in the curve itself, and only helps to shape it</param>
		/// <param name="p1">The second control point, and the start of the catrom curve</param>
		/// <param name="p2">The third control point, and the end of the catrom curve</param>
		/// <param name="p3">The last control point of the catrom curve. Note that this point is not included in the curve itself, and only helps to shape it</param>
		/// <param name="alpha">The alpha parameter controls how much the length of each segment should influence the knot values, which in turn influence the shape of the curve.
		/// A value of 0 is called a uniform catrom, and is fast to evaluate but has a tendency to overshoot.
		/// A value of 0.5 is a centripetal catrom, which follows points very tightly, and prevents cusps and loops.
		/// A value of 1 is a chordal catrom, which follows the points very smoothly with wide arcs</param>
		/// <param name="parameterizeToUnitInterval">If true, the knot generation will ensure k1 = 0 and k2 = 1,
		/// making it span the unit interval of 0 to 1 instead of using the raw knot values generated by the alpha parameterization</param>
		public NUCatRomCubic2D( Vector2 p0, Vector2 p1, Vector2 p2, Vector2 p3, float alpha, bool parameterizeToUnitInterval = true ) {
			pointMatrix = new Vector2Matrix4x1( p0, p1, p2, p3 );
			knotVector = default;
			validCoefficients = false;
			curve = default;
			knotCalcMode = parameterizeToUnitInterval ? KnotCalcMode.AutoUnitInterval : KnotCalcMode.Auto;
			this.alpha = alpha;
		}

		#endregion

		// serialized data
		[SerializeField] Vector2Matrix4x1 pointMatrix;
		public Vector2Matrix4x1 PointMatrix {
			get => pointMatrix;
			set => _ = ( pointMatrix = value, validCoefficients = false );
		}
		[SerializeField] Matrix4x1 knotVector;
		public Matrix4x1 KnotVector {
			get {
				if( knotCalcMode != KnotCalcMode.Manual )
					ReadyCoefficients();
				return knotVector;
			}
			set => _ = ( knotVector = value, validCoefficients = false );
		}

		// knot auto-calculation fields
		[SerializeField] KnotCalcMode knotCalcMode; // knot recalculation mode
		[SerializeField] float alpha; // alpha parameterization

		Polynomial2D curve;
		public Polynomial2D Curve {
			get {
				ReadyCoefficients();
				return curve;
			}
		}

		#region Properties

		/// <summary>The first control point of the catrom curve. Note that this point is not included in the curve itself, and only helps to shape it</summary>
		public Vector2 P0 {
			[MethodImpl( INLINE )] get => pointMatrix.m0;
			[MethodImpl( INLINE )] set => _ = ( pointMatrix.m0 = value, validCoefficients = false );
		}
		/// <summary>The second control point, and the start of the catrom curve</summary>
		public Vector2 P1 {
			[MethodImpl( INLINE )] get => pointMatrix.m1;
			[MethodImpl( INLINE )] set => _ = ( pointMatrix.m1 = value, validCoefficients = false );
		}
		/// <summary>The third control point, and the end of the catrom curve</summary>
		public Vector2 P2 {
			[MethodImpl( INLINE )] get => pointMatrix.m2;
			[MethodImpl( INLINE )] set => _ = ( pointMatrix.m2 = value, validCoefficients = false );
		}
		/// <summary>The last control point of the catrom curve. Note that this point is not included in the curve itself, and only helps to shape it</summary>
		public Vector2 P3 {
			[MethodImpl( INLINE )] get => pointMatrix.m3;
			[MethodImpl( INLINE )] set => _ = ( pointMatrix.m3 = value, validCoefficients = false );
		}

		/// <summary>The knot value of the first control point of the catrom curve</summary>
		public float K0 {
			[MethodImpl( INLINE )] get => KnotVector.m0;
			[MethodImpl( INLINE )] set => _ = ( knotVector.m0 = value, validCoefficients = false );
		}
		/// <summary>The knot value of the second control point, and the start of the catrom curve</summary>
		public float K1 {
			[MethodImpl( INLINE )] get => KnotVector.m1;
			[MethodImpl( INLINE )] set => _ = ( knotVector.m1 = value, validCoefficients = false );
		}
		/// <summary>The knot value of the third control point, and the end of the catrom curve</summary>
		public float K2 {
			[MethodImpl( INLINE )] get => KnotVector.m2;
			[MethodImpl( INLINE )] set => _ = ( knotVector.m2 = value, validCoefficients = false );
		}
		/// <summary>The knot value of the last control point of the catrom curve</summary>
		public float K3 {
			[MethodImpl( INLINE )] get => KnotVector.m3;
			[MethodImpl( INLINE )] set => _ = ( knotVector.m3 = value, validCoefficients = false );
		}

		/// <summary>The alpha parameter, which controls how much the length of each segment should influence the shape of the curve.
		/// A value of 0 is called a uniform catrom, and is fast to evaluate but has a tendency to overshoot.
		/// A value of 0.5 is a centripetal catrom, which follows points very tightly, and prevents cusps and loops.
		/// A value of 1 is a chordal catrom, which follows the points very smoothly with wide arcs</summary>
		public float Alpha {
			[MethodImpl( INLINE )] get => alpha;
			set => _ = ( alpha = value, validCoefficients = false );
		}

		#endregion

		// cached data to accelerate calculations
		[NonSerialized] bool validCoefficients; // inverted isDirty flag (can't default to true in structs)

		[MethodImpl( INLINE )] void ReadyCoefficients() {
			if( validCoefficients )
				return; // no need to update
			validCoefficients = true;
			if( knotCalcMode != KnotCalcMode.Manual )
				KnotVector = SplineUtils.CalcCatRomKnots( pointMatrix, alpha, knotCalcMode == KnotCalcMode.AutoUnitInterval );
			curve = SplineUtils.CalculateCatRomCurve( pointMatrix, knotVector );
		}

		/// <summary>Returns the weight of the given control point at the given parameter value</summary>
		/// <param name="i">The point to get the weight of</param>
		/// <param name="u">The parameter value at which to sample the weight</param>
		public float GetPointWeightAtKnotValue( int i, float u ) {
			float a = Mathfs.InverseLerp( K0, K1, u );
			float b = Mathfs.InverseLerp( K1, K2, u );
			float c = Mathfs.InverseLerp( K2, K3, u );
			float d = Mathfs.InverseLerp( K0, K2, u );
			float g = Mathfs.InverseLerp( K1, K3, u );
			switch( i ) {
				case 0:  return ( 1 - a ) * ( 1 - b ) * ( 1 - d );
				case 1:  return ( 1 - b ) * ( a * ( 1 - d ) + b * ( 1 - d - g ) + d );
				case 2:  return -b * ( b * ( d + g - 1 ) + g * ( c - 1 ) - d );
				case 3:  return b * c * g;
				default: throw new IndexOutOfRangeException( $"Catrom point has to be either 0, 1, 2 or 3. Got: {i}" );
			}
		}

	}

}